#include "Tangency.h"

namespace common {
Tangency::Tangency(int id,Point* k,Point* l,Point* m,Point* n):common::Constraint(id,1,TANGENCY){
	setK(k);
	setL(l);
	setM(m);
	setN(n);
}
ublasVector Tangency::getValue() throw(MyException){

	ublasVector o(1);
	Vector lk = (*getL()-*getK());
	Vector nm = (*getN()-*getM());
	Vector mk = (*getM()-*getK());

	o(0) = sqrt(pow(lk^mk,2)) - lk.length()*nm.length();
	return o;
}
ublasMatrix Tangency::getJacobianOnPoint(Point* p) throw(MyException){

	ublasMatrix mout(1,2);

	ublasMatrix R(ublasMatrixZero(2,2));
	R(0,1) = 1.0;
	R(1,0) = -1.0;
	ublasMatrix mR(R*(-1));

	Vector lk_ = (*getL()-*getK());
	Vector lm_ = (*getL()-*getM());
	Vector nm_ = (*getN()-*getM());
	Vector mk_ = (*getM()-*getK());

	ublasVector lk(2); ublasVector nm(2); ublasVector mk(2);ublasVector lm(2);
	ublasVector ulk(2); ublasVector unm(2);
	lk << lk_; nm << nm_; lm << lm_; mk << mk_;
	ulk << lk_.unit(); unm << nm_.unit();

	skreal g = lk_^mk_;
	skreal sg = 0;
	if(g!=0) sg = g/sqrt(g*g);

	if(k==p){
		row(mout,0)= sg*prod(R,lm) + nm_.length()*ulk;
	}
	else if(l==p){
		row(mout,0)= sg*prod(R,mk) - nm_.length()*ulk;
	}
	else if(m==p){
		row(mout,0)= sg*prod(mR,lk) + lk_.length()*unm;
	}
	else if(n==p){
		row(mout,0)= - lk_.length()*unm;
	}
	return mout;
}
ublasMatrix Tangency::getHessian(Point* s, Point* r) throw(MyException){

	ublasMatrix out(ublasMatrixZero(2,2));

	ublasMatrixIdentity I(2,2);
	ublasMatrix mI(-I);

	ublasMatrix R(ublasMatrixZero(2,2));
	R(0,1) = 1.0;
	R(1,0) = -1.0;
	ublasMatrix mR(R*(-1));

	Vector lk_ = (*getL()-*getK());
	Vector lm_ = (*getL()-*getM());
	Vector nm_ = (*getN()-*getM());
	Vector mk_ = (*getM()-*getK());

	ublasVector lk(2); ublasVector nm(2); ublasVector mk(2);ublasVector lm(2);
	ublasVector ulk(2); ublasVector unm(2);
	lk << lk_; nm << nm_; lm << lm_; mk << mk_;
	ulk << lk_.unit(); unm << nm_.unit();

	skreal g = lk_^mk_;
	skreal sg = 0;
	if(g!=0) sg = g/sqrt(g*g);

	skreal dlk = lk_.length();
	skreal dnm = nm_.length();

	if(k==s && l==r){
		out = sg*R;
	}else if(k==s && m==r){
		out = sg*mR - I*dnm*dlk;
	}else if(k==s && n==r){
		out = I*dnm*dlk;
	}else if(l==s && k==r){
		out = sg*mR;
	}else if(l==s && m==r){
		out = sg*R + I*dnm*dlk;
	}else if(l==s && n==r){
		out = mI*dnm*dlk;
	}else if(m==s && k==r){
		out = sg*R - I*dnm*dlk;
	}else if(m==s && l==r){
		out = sg*mR + I*dnm*dlk;
	}else if(n==s && k==r){
		out = I*dnm*dlk;
	}else if(n==s && l==r){
		out = mI*dnm*dlk;
	}else {
		return ublasMatrixZero(2,2);
	}
	return out;
}
bool Tangency::isHessian() const{
	return true;
}
Tangency::~Tangency(){

}

}
